Dead reckoning-augmented GPS for tracked vehicles

ABSTRACT

The invention relates to an apparatus and method for augmenting the 3 dimensional position information obtained from the NAVSTAR satellite-based global positioning system (“GPS”) system. Such systems can be impacted by physical obstacles that prevent the receipt of the satellite signals or as a result of sun spot activity that introduces noise into the signals thus causing them to become intermittently unavailable and/or making them less accurate in the course of normal operation. Therefore, an improved positioning solution that can operate under such poor GPS operational conditions is needed. The apparatus and method of the invention augments GPS with dead reckoning techniques when GPS signals are unavailable or inaccurate. The apparatus and method of the invention demonstrates highest value when applied to blasthole drill positioning applications in open-pit mines.

RELATED APPLICATIONS

This application is a divisional of U.S. patent application Ser. No.15/031,179, filed Apr. 21, 2016, which is a U.S. national stageapplication of International Application No. PCT/CA2014/000755, filed onOct. 21, 2014, which claims the benefit of U.S. Provisional ApplicationNo. 61/895,342, filed on Oct. 24, 2013. The entire contents of thoseapplications are incorporated herein by reference.

TECHNICAL FIELD

The invention relates to an apparatus and method for augmenting thethree-dimensional position information obtained from the NAVSTARsatellite-based global positioning system (“GPS”) system. The apparatusand method demonstrates highest value when providing augmented positioninformation to high-precision GPS-based guidance systems on blastholedrills that are routinely used in open pit (surface) mines.

BACKGROUND

Dead reckoning is a navigational technique which has been in use forcenturies. Dead reckoning calculates the current position of an objectbased on a previous position of the object in view of the speed anddirection traveled from the previous position. Disadvantageously, deadreckoning is subject to significant error, particularly when speed anddirection are not measured accurately.

The NAVSTAR (US government owned and operated) GPS constellationcomprises a network of 27 Earth orbiting satellites. A complementaryspace-based network called. GLONASS (Russian government owned andoperated) consists of an additional 24 satellites. In order to determinethe position of an object using GPS/GLONASS, a GPS, GLONASS or combinedGPS/GLONASS receiver on the object must determine the location of atleast four GPS/GLONASS satellites and the distance between the objectand each of the at least four satellites. Disadvantageously, theGPS/GLONASS system cannot be used to calculate position when theGPS/GLONASS receiver does not receive signals from at least fourGPS/GLONASS satellites.

The introduction of high-precision global positioning systems (“HPGPS”)to the surface mining industry has resulted in significant improvementsin productivity, and is expected to take an essential role as anenabling technology in future efforts to automate mining activities. Ina standard system, GPS/GLONASS output is used directly for positioning.However, such systems can be impacted by physical obstacles that preventthe receipt of the satellite signals or as a result of sun spot activitythat introduces noise into the signals thus causing them to becomeintermittently unavailable and/or making them less accurate in thecourse of normal operation. Therefore, an improved positioning solutionthat can operate under such poor GPS operational conditions is needed.The apparatus and method of the invention augments GPS with deadreckoning techniques when GPS signals are unavailable or inaccurate.

SUMMARY OF THE INVENTION

The invention relates to an augmented GPS (“aGPS”) apparatus and method,which alleviates the availability problem of a GPS receiver only (doesnot take into account a loss of GLONASS receiver signal) by combining itwith dead reckoning techniques. The invention may be used in relation toa number of vehicle types (for example; tracked vehicles such asblasthole drills, excavators and bulldozers, or rubber tired vehiclessuch as haul-trucks and graders). The invention is particularly suitedto blasthole drills. During operation, a blasthole drill typicallytravels for two minutes, stops and has its jacks lowered, drills forbetween thirty (30) and sixty (60) minutes, has its jacks retracted andtravels an additional two (2) minutes where the process is repeated. Inmost cases, it is desirable for the blasthole drill to travel in astraight line for an extended period of time and distance.

The augmented GPS of the invention introduces an intermediate stepbetween the GPS receiver and the machine. Under normal conditions, theGPS output of the invention is identical to a GPS system. However, underdegrading space-based GPS satellite conditions, the system insteadestimates the motion of the machine based on local sensor measurements,and uses this to extrapolate the last known GPS position. Thisconstructed position is output in place of the unavailable GPS position.This process of extrapolation continues until either the GPS situationimproves, or the uncertainty in the constructed position exceeds apredefined maximum allowable value. The target precision of this systemis to estimate the vehicle position within six (6) inches of its truevalue over a traveled distance of one hundred (100) feet (0.5%).

In accordance with one aspect of the present invention, there isprovided an augmented global positioning system (“aGPS”) for a vehiclecomprising:

-   -   (a) an aGPS computer;    -   (b) a standard global positioning system (“GPS”) system        operatively connected to said aGPS computer, the standard GPS        comprising:        -   (i) a high-precision GPS receiver;        -   (ii) a navigation system; and        -   (iii) a switch for alternating between a use of the standard            GPS and the aGPS; and    -   (c) a chorus subsystem operatively connected to the standard        GPS, the chorus subsystem comprising:        -   (i) a chorus data acquisition (DAQ) module;        -   (ii) a gyroscope operatively connected to the chorus DAQ;            and        -   (iii) at least two rotation sensors operatively connected to            the chorus DAQ.

In accordance with another aspect of the present invention, there isprovided a method for determining the position of a moving vehicle usingaugmenting global positioning system (“aGPS”), the method comprising thesteps of:

-   -   (a) calculating a first position of the vehicle using a global        positioning system (“GPS”);    -   (b) upon losing the GPS signal, measuring the movement of the        vehicle and calculating the position of the vehicle using the        last known position of the vehicle from the GPS combined with        dead reckoning;    -   (c) upon reacquiring a GPS signal, comparing the first position        of the vehicle to the calculated position; and    -   (d) correcting error in said calculated position.

BRIEF DESCRIPTION OF THE FIGURES

FIG. 1 is a schematic showing a first embodiment of the invention;

FIG. 2 is a prospective side view showing an exemplary mounting locationof a wheel sensor assembly; and

FIG. 3 is a flow chart showing a method of augmenting cording to anembodiment of the invention.

DETAILED DESCRIPTION

Referring to FIG. 1, a first embodiment of a high-availability globalpositioning system with local sensor augmentation (10) of the inventionis shown. The system (10) is preferably used with blasthole drills, forexample the Atlas Copco™ PV-271 blasthole drill. The system comprises astandard GPS system (20), an aGPS computer (30), and a chorus subsystem(40).

The standard GPS system (20) comprises a dual-antenna high-precision GPSreceiver (22), a navigation system (24), and a switch (26). The switch(26) allows the system to alternate between operation when a GPS signalis available, during which time the standard GPS system (20) is used,and when the GPS signal is not available, during which time the chorussubsystem (40) is used.

The aGPS computer (30) acts as the processing unit for system (10),receiving sensor data as input and producing vehicle positioninformation as output.

The chorus subsystem (40) comprises a left rotation sensor (42), a rightrotation sensor (44), and a chorus enclosure (50). Preferably, the leftrotation sensor (42) and the right rotation sensor (44) are rotaryencoders capable of measuring angular positions of the left and rightwheels of the vehicle. The sensors (42) and (44) use a polarizedmagnet-sensor pair to sense the angular positions of the left and rightdrive motors, which directly drive the vehicle's crawler tracks. Fromsensors (42) and (44), the distance traveled by the vehicle is measured.The chorus enclosure (50) comprises a gyroscope (52) and a chorus dataacquisition module (54). The gyroscope (52) obtains angular ratemeasurements about the vehicle's turning axis of rotation. For example,the gyroscope may be an ADIS16130 single-axis MEMS gyroscope produced byAnalog Devices M. The chorus data acquisition module (54) comprises asupporting hardware unit which forwards sensor measurements from theleft rotation sensor (42), right rotation sensor (44), and the gyroscope(52) to the aGPS computer (30).

Referring to FIG. 2, an exemplary mounting location of a wheel sensorassembly is shown. A magnetic wheel sensor assembly (60) is shown inassociation with a hydraulic propel motor (62) of a crawler track (64).The magnetic wheel sensor assembly (60) consists of a polarized magnet(66) and a nearby magnetic pickup sensor (68). The sensor is preferablya two-axis magnetometer (essentially a digital compass). The magnet (66)is rigidly attached to the wheel and rotates with it, thus the magnet's“north” rotates with the wheel. The sensor is able to sense thedirection of this magnetic “north” as it rotates, thus providing aninstantaneous angular position of the wheel. Alternatively, rotaryencoders of any type capable of the required precision may besubstituted for the magnet-based sensors.

Referring to FIG. 3, a method of the present invention is shown. Duringnormal operation, a vehicle receives positional information from thestandard GPS system (20) (Step 100). However, upon losing the GPSsignal, the movement of the vehicle is measured and the new vehicleposition is calculated in using chorus subsystem (40) (Step 200). ThisStep 200 comprises measuring the distance the vehicle has traveled usingat least two wheel sensors. Step 200 further comprises measuring thedirection the vehicle has traveled. Preferably, this is performed usingat least one gyroscope (52). Step 200 may be repeated as necessary inresponse to intermittent GPS signals. Upon reacquiring a GPS signal, thefirst position of the vehicle is compared to the calculated position ofStep 200 and any error in the calculated position is corrected (Step300). Alternatively, the process may stop when the calculated positionexceeds a predefined maximum allowable value (Step 400).

The aGPS computer (30) contains a filter algorithm in order to maintainan optimal estimate of the position and orientation of the vehicle as ittravels from point to point. The filter is an unscented Kalman filter(UKF)-based design incorporating wheel rotation sensors (42, 44), agyroscope (52), and a HPGPS (22) which is intermittently unavailable.

Nomenclature

In the following description, capital letters are used to denotequantities in an absolute “world” reference frame and lowercase lettersto denote those in other reference frames. The global frame is aCartesian frame predefined by the mine site and measured in metres. Minesite coordinates are specified in terms of a Northing (metres in the Ndirection), Easting (metres in the E direction), and an ellipsoidalheight. For convenience, the “world frame” is a right-handed 3-DCartesian frame comprising (X, Y, Z), where X is in the direction of theEasting, Y is in the direction of the Northing, and Z points upward andis related to the ellipsoidal height.

A vehicle's local frame is defined similarly. It is a right-handedCartesian frame comprising (x,y,z), where x is measured in the vehicle's“forward” direction, y is measured in the “leftward” direction, and z inthe upward direction. The vehicle's frame is defined to be directlybetween the track midpoints, at ground level. Orientations are specifiedin terms of the coordinate axes. Rotations and orientations about theworld frame's (X, Y, Z) axes are denoted Θ_(X), Θ_(Y), Θ_(Z)respectively. Similarly, in the vehicle frame, θ_(X), θ_(Y), θ_(Z) areused for orientations. A hat (′) is used to denote an estimatedquantity.

State

Assuming the vehicle travels in a 2-D plane, only a subset of statevariables are needed to achieve the desired accuracy. The state to beestimated is denoted q and consists of the global position andorientation of the vehicle's frame. It is represented as:

$q = {\begin{bmatrix}X \\Y \\\Theta_{Z}\end{bmatrix}.}$The state q has an associated 3×3 covariance matrix P.Initialization

The above filter must be initialized using an absolute coordinatereference. Initialization can occur when two conditions aresimultaneously met;

-   -   1. an RTK GPS fix is available. With this, the state variables X        and Y can be initialized with the vehicle's current location in        the absolute world coordinate frame; and    -   2. the vehicle is moving in a straight line, either forward or        reverse. Since a single-antenna GPS receiver cannot measure its        orientation, a heading is constructed based on consecutive GPS        readings as detailed in the section entitled “Absolute Heading        Estimate”, below.

Since the RTK fix is not always available and since the vehicle spendsmost of its time stationary, it can take a long time for the above twoconditions to be met under normal operating conditions. However, thiscan be remedied by making use of dual-antenna GPS hardware. Theprovision of a dual-antenna GPS hardware would remove condition “2” andallow the filter to initialize any time the RTK fix is available,regardless of the vehicle's motion.

Absolute Heading Estimate

While in theory, the GPS can reports its orientation via the HDTmessage, this is not a viable option likely due to the low speed of thedrill. As an alternative, a heading can be constructed using the outputGPS coordinates while the vehicle is moving.

If the vehicle is moving, assuming two consecutive GPS coordinatereadings (X₁, Y₁) with uncertainty (σ_(X1), σ_(Y1)) and (X₂, Y₂) withuncertainty (σ_(X2), σ_(Y2)), the heading Θ_(Z) can be computed as

${\beta = \frac{Y_{2} - Y_{1}}{X_{2} - X_{1}}},{\Theta_{Z} = {{\arctan(\beta)}.}}$Using the standard error propagation formula, e uncertainties are

${\sigma_{\beta} = \sqrt{{\frac{1}{\left( {X_{2} - X_{1}} \right)^{2}}\left( {\sigma_{Y\; 2}^{2} + \sigma_{Y\; 1}^{2}} \right)} + {\frac{\left( {Y_{2} - Y_{1}} \right)^{2}}{\left( {X_{2} - X_{1}} \right)^{4}}\left( {\sigma_{X\; 2}^{2} + \sigma_{X\; 1}^{2}} \right)}}},{and}$$\sigma_{\Theta_{Z}} = {\frac{\sigma_{\beta}}{1 + \beta^{2}}.}$Since the above process implicitly assumes that the vehicle is moving ina straight line (i.e. {dot over (Θ)}_(Z)=0), an addition errorcomponent, σ_(m), is defined to account for error due to movement duringthe measurement. This additional error can be expressed as:

${\sigma_{m} = \frac{d_{R} - d_{L}}{W}},$where d_(R) and d_(L) are the differential distances moved by the tracksduring the measurement interval, and W is the distance between thetracks. If the vehicle is actually moving in a straight line, thend_(R)≈d_(L) and σ_(m)≈0. Thus, σ_(θ) _(Z) is defined to be:

$\sigma_{\Theta_{Z}} = {\frac{\sigma_{\beta}}{1 + \beta^{2}} + {\sigma_{m}.}}$

A number of conditions on the input data are enforced before applyingthe above procedure to construct a heading estimate. If any of theseconditions fail, no computed heading is available. The conditions are:

-   -   1. Both GPS data points (X₁, Y₁) and (X₂, Y₂) must have RTK        precision.    -   2. There is a minimum distance between the GPS data points. The        distance d is computed using the formula:        d=√{square root over ((X ₂ −X ₁)²+(Y ₂ −Y ₁)²)}

The threshold used is d_(min)=0.1 m. Thus, this condition is met ifd≥d_(min).

-   -   3. The track speed of the left and right tracks must be similar.        This confirms that the drill is travelling in a straight line,        either forward or backward. The distance traveled by each track        during the interval between data points denoted dr_(L) and        dr_(R) are computed using the difference sin angular values with        a constant found by calibration. The absolute value of their        difference Δdr is then compared against a threshold value        Δdr_(max).        Absolute Position Estimate

The absolute position X, Y, Z is obtained directly from the HPGPS' PTNL,PJK message. Since the GPS is not located at the defined machine origin,the reported values must be transformed into the machine frame using themost recent estimate of Θ_(Z). Defining the offset of the GPS antenna inthe vehicle's frame as (x_(GPS), y_(GPS)), the absolute position of theGPS is

$\begin{matrix}{\begin{bmatrix}X_{GPS} \\Y_{GPS}\end{bmatrix} = {\begin{bmatrix}X \\Y\end{bmatrix} + {\begin{bmatrix}{\cos\mspace{11mu}\Theta_{Z}} & {{- \sin}\mspace{11mu}\Theta_{Z}} \\{\sin\mspace{11mu}\Theta_{Z}} & {\cos\mspace{11mu}\Theta_{Z}}\end{bmatrix}\begin{bmatrix}x_{GPS} \\y_{GPS}\end{bmatrix}}}} & (1)\end{matrix}$

The corresponding covariance P is obtained directly from the GSTmessage. This formulation can be extended to the full 3D case later ifnecessary.

Vehicle Kinematic Model

A tracked vehicle is modelled as a differential-drive vehicle with twowheels separated by a distance W. Using measurements from the wheelencoders and an experimentally-determined calibration constant, thedifferential distances each track has moved since the last step can bemeasured. For the right and left tracks respectively, these are Δr_(R)and Δr_(L). The updated equation is

$\begin{matrix}\begin{matrix}{q_{k + 1} = {q_{k} + {G_{s,k}u_{k}}}} & {{~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~}(2)} \\{{= {\begin{bmatrix}x_{k} \\y_{k} \\\theta_{z,k}\end{bmatrix} + {{\begin{bmatrix}{0.5\cos\mspace{11mu}\theta_{z,k}} & {0.5\cos\mspace{11mu}\theta_{z,k}} \\{0.5\sin\mspace{11mu}\theta_{z,k}} & {0.5\sin\mspace{11mu}\theta_{z,k}} \\{1/W} & {{- 1}/W}\end{bmatrix}\begin{bmatrix}{\Delta\; r_{R}} \\{\Delta\; r_{L}}\end{bmatrix}}.}}}\mspace{14mu}} & {(3)}\end{matrix} & \mspace{205mu}\end{matrix}$State Update

The filter's UKF-based estimation algorithm uses the familiarpredict-update cycle to maintain its state estimate.

The prediction (a-priori) step is always done and is based on deadreckoning measurements. The basic premise is to use the kinematic model,described above, in a UKF a-priori step with a modificationincorporating both wheel encoders and the z-gyro as measurements forrotation, First, the rotation due to wheels Δθ_(w) and the uncertaintyσ_(w) of the same is defined:

${\Delta\sigma}_{w} = \frac{{\Delta\; r_{L}} - {\Delta\; r_{R}}}{W}$${\sigma_{w} = \sqrt{\frac{\left( {{F_{w}\Delta\; r_{R}} + C_{w}} \right)^{2} + \left( {{F_{w}\Delta\; r_{L}} + C_{w}} \right)^{2}}{W}}},$where F_(w) and C_(w) are constants. Next a simple condition is used todetermine whether the vehicle is currently moving:

$\frac{{\Delta\; r_{R}} + {\Delta\; r_{L}}}{2} \geq D_{\min}$where D_(min) is a constant threshold. Depending on whether thecondition (4) is true, one of the following is performed:

-   -   1. If (4) is true, the vehicle is moving. Thus a rotation        measurement is obtained from the z-gyro:        Δθ_(g) =Tg _(z) −b _(z)        σ_(g) =S _(g),        -   where g_(z) is the current raw measurement from the            gyroscope units of rad/s), T is the timestep, b_(z) is the            constant gyro bias (discussed below in step “2”), and S_(g)            is the constant uncertainty of the gyro measurements. Next            the combined equivalent measurement and uncertainty as the            uncertainty-weighted mean of the values from the wheels and            the gyro is calculated:

$\sigma_{c} = {{{\frac{1}{\frac{1}{\sigma_{w}^{2}} + \frac{1}{\sigma_{g}^{2}}}.\Delta}\;\theta_{c}} = {{\sigma_{c}\left( {\frac{{\Delta\sigma}_{w}}{\sigma_{w}^{2}} + \frac{\Delta\;\theta_{g}}{\sigma_{g}^{2}}} \right)}.}}$

-   -   -   With the uncertainties correctly adjusted, this scheme tends            to trust the gyroscope measurements more while moving, and            the wheel sensor measurements while moving slowly or            stationary.

    -   2. If (4) is false, we consider the vehicle to be stationary. In        this case, the combined rotation values are those of the wheels        alone:        Δθ_(c)=Δθ_(w)        σ_(c)=σ_(w).        Since the gyroscope's bias b_(z) drifts over time, the        (stationary) time can be used to estimate its current value. A        (normal) Kalman filter is used to track both the gyroscope bias        b_(z) and its uncertainty σ_(b), The expression Δθ_(g)−Δθ_(w)        represents a measurement of its current value, and incorporates        it into b_(z) using one step of the Kalman filter. This filter        is effectively only an a-posteriori step.

The a-priori step is then done using an unscented transformation, andincorporating Δθ_(c) in place of Δθ_(w) in the deconstructed model (2).

The update step is done according to one of the cases below.

-   -   1. Case 1: RTK fix available and the conditions of “Absolute        Heading Estimation” are fulfilled. In such a case, a heading is        constructed as detailed in that section. The machine state q is        transformed into the GPS frame using the inverse of Equation        (1), and a full-state update is done in the GPS frame using the        usual UKF update step with the recent GPS position and        constructed heading.    -   2. Case 2: RTK fix available, but the conditions of “Absolute        Heading Estimate” are not fulfilled. In such a case, the machine        state q is transformed into the GPS frame using the inverse of        Equation (1), and a partial-state update is done in the GPS        frame using the usual UKF update step with the recent GPS (X,Y)        position.    -   3. Case 3: RTK is not available, in which case, the update step        is skipped.

A person of skill in the art would recognize that the type, number, andposition of said sensors and gyroscope may be varied according to theintended use.

The scope of the claims should not be limited by the preferredembodiments set forth in the examples, but should be given the broadestinterpretation consistent with the description as a whole.

The invention claimed is:
 1. A method for determining a position of amoving vehicle using augmenting global positioning system (“aGPS”), themethod comprising the steps of: (a) calculating a first position of thevehicle using a global positioning system (“GPS”); (b) upon losing theGPS signal, measuring the movement of the vehicle and calculating theposition of the vehicle using thea last known position of the vehiclefrom the GPS combined with dead reckoning; (c) upon reacquiring a GPSsignal, comparing the first position of the vehicle to the calculatedposition; and (d) correcting error in said calculated position.
 2. Themethod of claim 1, wherein the steps of (b) to (d) are repeated.
 3. Themethod of claim 1, wherein the steps of measuring the movement of thevehicle comprises measuring a distance the vehicle has travelled usingat least one sensor.
 4. The method of claim 3, wherein the at least onesensor comprises at least one wheel sensor.
 5. The method of claim 1,wherein the step of measuring the movement of the vehicle comprisesmeasuring a direction the vehicle has travelled using at least onegyroscope.